#!/bin/zsh
# Enhanced LLM Voice Control System 启动脚本 (macOS版本)
# 基于 start_microros_voice_system.sh 模板，集成 LLM 增强功能
# 适用于 macOS，支持增强版 LLM 桥接语音控制节点

set -e

GREEN='\033[0;32m'
YELLOW='\033[1;33m'
RED='\033[0;31m'
BLUE='\033[0;34m'
PURPLE='\033[0;35m'
CYAN='\033[0;36m'
NC='\033[0m'

echo -e "${PURPLE}╔══════════════════════════════════════════════════════════════╗${NC}"
echo -e "${PURPLE}║     Enhanced LLM Voice Control System (macOS)               ║${NC}"
echo -e "${PURPLE}║     增强版 LLM 语音控制系统 - macOS 优化版                    ║${NC}"
echo -e "${PURPLE}╚══════════════════════════════════════════════════════════════╝${NC}"
echo ""

# 检测操作系统和Shell
OS=$(uname -s)
echo -e "${CYAN}🖥️  操作系统: $OS${NC}"
echo -e "${CYAN}🐚 Shell: $SHELL${NC}"

if [[ "$OS" != "Darwin" ]]; then
    echo -e "${YELLOW}⚠️  警告: 此脚本专为 macOS 优化，当前系统为 $OS${NC}"
fi

# 设置环境 - macOS 优化
echo -e "${YELLOW}🔧 配置 ROS2 环境...${NC}"

# 检查 ROS2 环境
if ! command -v ros2 &> /dev/null; then
    echo -e "${RED}❌ ROS2 未找到，请确保已安装并激活ROS2环境${NC}"
    exit 1
fi

# 尝试加载 ROS2 环境 (支持多版本)
if [[ -f "/opt/ros/jazzy/setup.zsh" ]]; then
    source /opt/ros/jazzy/setup.zsh
    echo -e "${GREEN}✅ 加载 ROS2 Jazzy (zsh)${NC}"
elif [[ -f "/opt/ros/humble/setup.zsh" ]]; then
    source /opt/ros/humble/setup.zsh
    echo -e "${GREEN}✅ 加载 ROS2 Humble (zsh)${NC}"
elif [[ -f "/opt/ros/jazzy/setup.bash" ]]; then
    source /opt/ros/jazzy/setup.bash
    echo -e "${GREEN}✅ 加载 ROS2 Jazzy (bash)${NC}"
elif [[ -f "/opt/ros/humble/setup.bash" ]]; then
    source /opt/ros/humble/setup.bash
    echo -e "${GREEN}✅ 加载 ROS2 Humble (bash)${NC}"
else
    echo -e "${YELLOW}⚠️  未找到标准 ROS2 安装，假设环境已配置${NC}"
fi

# 设置 ROS 域 ID
export ROS_DOMAIN_ID=0
echo -e "${GREEN}✅ ROS_DOMAIN_ID 设置为 0${NC}"

# 切换到脚本目录
cd "$(dirname "$0")"

# 构建包 - macOS 优化
echo ""
echo -e "${YELLOW}🔨 构建 fishbot_microros_voice 包...${NC}"
colcon build --packages-select fishbot_microros_voice

if [ $? -eq 0 ]; then
    echo -e "${GREEN}✅ 包构建成功${NC}"
else
    echo -e "${RED}❌ 包构建失败${NC}"
    exit 1
fi

# 加载工作空间环境 - macOS 使用 zsh
echo -e "${YELLOW}🔧 加载工作空间环境...${NC}"
if [[ -f "install/setup.zsh" ]]; then
    source install/setup.zsh
    echo -e "${GREEN}✅ 工作空间环境已加载 (zsh)${NC}"
elif [[ -f "install/setup.bash" ]]; then
    source install/setup.bash
    echo -e "${GREEN}✅ 工作空间环境已加载 (bash)${NC}"
else
    echo -e "${RED}❌ 工作空间环境文件未找到${NC}"
    exit 1
fi

# 显示系统架构
echo ""
echo -e "${BLUE}🔧 Enhanced LLM 系统架构:${NC}"
echo -e "${CYAN}   [EchoKit Box] → [EchoKit Server LLM:8060] → [Enhanced LLM Bridge Node] → [ROS2 Topics] → [micro-ros-agent:8888] → [EchokitBot UDP]${NC}"
echo ""

# 1. 检查 micro-ros-agent
echo -e "${YELLOW}🤖 检查 micro-ros-agent 状态...${NC}"

# 检查 Docker 是否可用
if command -v docker &> /dev/null; then
    if docker ps | grep -q micro-ros-agent; then
        echo -e "${GREEN}✅ micro-ros-agent 运行正常${NC}"
        docker ps --filter name=micro-ros-agent --format "   容器: {{.Names}} | 状态: {{.Status}}"
    else
        echo -e "${YELLOW}⚠️  micro-ros-agent 未运行${NC}"
        echo -e "${CYAN}💡 建议手动启动 micro-ros-agent:${NC}"
        echo -e "${CYAN}   docker run -d --name micro-ros-agent --restart=unless-stopped --net=host microros/micro-ros-agent:jazzy udp4 --port 8888 -v6${NC}"
    fi
else
    echo -e "${YELLOW}⚠️  Docker 未安装或不可用${NC}"
    echo -e "${CYAN}💡 假设 micro-ros-agent 已在 8888 端口手动启动${NC}"
fi

# 2. 检查 EchoKit Server
echo ""
echo -e "${YELLOW}🧠 检查 EchoKit Server LLM 状态...${NC}"

# 设置 EchoKit 服务器地址 (支持环境变量)
ECHOKIT_IP=${ECHOKIT_SERVER_IP:-"192.168.2.28"}
ECHOKIT_PORT=${ECHOKIT_SERVER_PORT:-"8060"}

echo -e "${CYAN}📡 服务器地址: ${ECHOKIT_IP}:${ECHOKIT_PORT}${NC}"

# 检查 HTTP 端口
if curl -s --connect-timeout 3 http://${ECHOKIT_IP}:${ECHOKIT_PORT}/health > /dev/null 2>&1; then
    echo -e "${GREEN}✅ EchoKit Server HTTP 端口 (${ECHOKIT_IP}:${ECHOKIT_PORT}) 运行正常${NC}"
else
    echo -e "${YELLOW}⚠️  EchoKit Server HTTP 端口 (${ECHOKIT_IP}:${ECHOKIT_PORT}) 不可达${NC}"
    echo -e "${CYAN}   请确保 EchoKit Server 正在运行${NC}"
fi

# 检查 WebSocket 端口
if timeout 3 bash -c "</dev/tcp/${ECHOKIT_IP}/${ECHOKIT_PORT}" 2>/dev/null; then
    echo -e "${GREEN}✅ EchoKit Server WebSocket 端口 (${ECHOKIT_IP}:${ECHOKIT_PORT}) 可连接${NC}"
else
    echo -e "${YELLOW}⚠️  EchoKit Server WebSocket 端口 (${ECHOKIT_IP}:${ECHOKIT_PORT}) 不可达${NC}"
    echo -e "${CYAN}   请确保 EchoKit Server 正在运行并监听 ${ECHOKIT_IP}:${ECHOKIT_PORT} 端口${NC}"
fi

# 检查 LLM 配置文件
echo -e "${YELLOW}🔍 检查 EchoKit LLM 配置...${NC}"

# 可能的配置文件路径
CONFIG_PATHS=(
    "/Users/vesperxiao/rust/echokit_server0907_robot/config.toml"
    "/home/hans/echokit_server0907_robot/config.toml"
    "./echokit_server_llm_config.toml"
    "./optimized_server_config.toml"
    "./fixed_server_config.toml"
)

CONFIG_FOUND=false
for config_path in "${CONFIG_PATHS[@]}"; do
    if [[ -f "$config_path" ]]; then
        echo -e "${GREEN}✅ EchoKit 配置文件存在: $config_path${NC}"
        CONFIG_FOUND=true
        
        # 检查 LLM 增强配置
        if grep -q "structured JSON commands\|llm_enhanced\|MessagePack" "$config_path" 2>/dev/null; then
            echo -e "${GREEN}✅ 配置文件包含 LLM 增强设置${NC}"
        elif grep -q "\[forward\]" "$config_path" 2>/dev/null; then
            echo -e "${CYAN}✅ 配置文件包含方括号格式命令${NC}"
        else
            echo -e "${YELLOW}⚠️  配置文件可能需要 LLM 增强配置${NC}"
        fi
        break
    fi
done

if [[ "$CONFIG_FOUND" == false ]]; then
    echo -e "${YELLOW}⚠️  未找到 EchoKit 配置文件${NC}"
    echo -e "${CYAN}💡 建议检查配置文件路径或使用提供的配置模板${NC}"
fi

# 3. 检查 EchokitBot 网络
echo ""
echo -e "${YELLOW}🤖 检查 EchokitBot 网络连通性...${NC}"
if ping -c 2 192.168.2.42 > /dev/null 2>&1; then
    echo -e "${GREEN}✅ EchokitBot (192.168.2.42) 网络可达${NC}"
else
    echo -e "${YELLOW}⚠️  EchokitBot (192.168.2.42) 网络不可达${NC}"
    echo -e "${CYAN}   请确保 EchokitBot 已开机并连接到网络${NC}"
fi

# 4. 检查可执行文件
echo ""
echo -e "${YELLOW}📦 检查增强版节点可执行文件...${NC}"
EXECUTABLE_PATH="./install/fishbot_microros_voice/bin/llm_bridge_voice_control_node"

if [[ -f "$EXECUTABLE_PATH" ]]; then
    echo -e "${GREEN}✅ 增强版节点可执行文件存在${NC}"
    echo -e "${CYAN}   路径: $EXECUTABLE_PATH${NC}"
    echo -e "${CYAN}   权限: $(ls -la "$EXECUTABLE_PATH" | awk '{print $1}')${NC}"
else
    echo -e "${RED}❌ 增强版节点可执行文件不存在${NC}"
    echo -e "${CYAN}💡 尝试使用标准启动方式${NC}"
fi

# 5. 启动增强版 LLM 语音控制系统
echo ""
echo -e "${YELLOW}🎯 启动 Enhanced LLM 语音控制系统...${NC}"
echo -e "${CYAN}配置:${NC}"
echo -e "${CYAN}  - EchoKit Server LLM: ws://${ECHOKIT_IP}:${ECHOKIT_PORT}/ws/robot${NC}"
echo -e "${CYAN}  - micro-ros-agent: localhost:8888 (UDP)${NC}"
echo -e "${CYAN}  - EchokitBot: 192.168.2.42 (通过 micro-ROS)${NC}"
echo ""

echo -e "${BLUE}✨ Enhanced LLM 功能特性:${NC}"
echo -e "${GREEN}  🧠 LLM 智能语音理解和命令解析${NC}"
echo -e "${GREEN}  📦 MessagePack 和 JSON 双格式支持${NC}"
echo -e "${GREEN}  🔄 智能消息重组和分割处理${NC}"
echo -e "${GREEN}  🛡️ 高级安全机制 (命令超时、紧急停止、速度限制)${NC}"
echo -e "${GREEN}  🔧 完整的 ROS2 参数系统${NC}"
echo -e "${GREEN}  📊 性能监控和状态发布${NC}"
echo -e "${GREEN}  🔗 持久连接和自动重连机制${NC}"
echo -e "${GREEN}  ⚡ 优化的命令执行时序${NC}"
echo ""

echo -e "${BLUE}🎤 支持的智能语音命令:${NC}"
echo -e "${GREEN}  📐 精确角度控制: '左转90度', '右转180度', '左转45度'${NC}"
echo -e "${GREEN}  📏 精确距离控制: '前进2米', '后退1.5米', '前进0.5米'${NC}"
echo -e "${GREEN}  ⏱️  精确时间控制: '左转3秒', '前进5秒', '右转2秒'${NC}"
echo -e "${GREEN}  🔄 基础控制: '前进', '后退', '左转', '右转'${NC}"
echo -e "${GREEN}  🛑 停止控制: '停止'${NC}"
echo -e "${GREEN}  🌐 多语言支持: 中文、英文${NC}"
echo -e "${GREEN}  📋 结构化命令: JSON 格式、MessagePack 格式${NC}"
echo ""

echo -e "${BLUE}📡 ROS2 话题监控:${NC}"
echo -e "${CYAN}  - /cmd_vel (机器人运动控制)${NC}"
echo -e "${CYAN}  - /voice_text (语音文本输出)${NC}"
echo -e "${CYAN}  - /structured_voice_command (结构化命令)${NC}"
echo -e "${CYAN}  - /voice_control_status (状态信息)${NC}"
echo -e "${CYAN}  - /voice_control/status (兼容状态)${NC}"
echo -e "${CYAN}  - /voice_control/feedback (语音反馈)${NC}"
echo -e "${CYAN}  - /voice_control/mode (控制模式)${NC}"
echo ""

echo -e "${BLUE}🤖 系统将自动:${NC}"
echo -e "${GREEN}  ✅ 连接到 EchoKit Server LLM WebSocket${NC}"
echo -e "${GREEN}  ✅ 解析多种消息格式 (MessagePack/JSON)${NC}"
echo -e "${GREEN}  ✅ 执行精确运动控制${NC}"
echo -e "${GREEN}  ✅ 发布结构化命令到 ROS2 话题${NC}"
echo -e "${GREEN}  ✅ 监控连接状态和安全检查${NC}"
echo -e "${GREEN}  ✅ 自动停止和安全保护${NC}"
echo ""

# 启动系统
echo -e "${GREEN}🚀 启动 Enhanced LLM Voice Control System...${NC}"
echo ""
echo -e "${YELLOW}🎯 启动模式: 增强版单节点 (macOS 优化)${NC}"
echo -e "${YELLOW}按 Ctrl+C 停止系统${NC}"
echo ""

# 设置服务器 URL
ECHOKIT_SERVER_URL="ws://${ECHOKIT_IP}:${ECHOKIT_PORT}/ws/robot"
echo -e "${GREEN}使用 EchoKit 服务器地址: ${ECHOKIT_SERVER_URL}${NC}"
echo ""

# 尝试多种启动方式 (macOS 优化)
echo -e "${CYAN}🔧 启动方式检测...${NC}"

# 方式1: 尝试使用 ros2 run (可能在 macOS 上不工作)
if command -v llm_bridge_voice_control_node &> /dev/null; then
    echo -e "${CYAN}方式1: 使用 PATH 中的可执行文件${NC}"
    llm_bridge_voice_control_node
elif [[ -f "$EXECUTABLE_PATH" ]]; then
    echo -e "${CYAN}方式2: 使用直接路径执行${NC}"
    "$EXECUTABLE_PATH"
else
    # 方式3: 尝试使用 launch 文件 (如果存在)
    LAUNCH_FILE="src/fishbot_microros_voice/launch/llm_enhanced_voice_system.launch.py"
    if [[ -f "$LAUNCH_FILE" ]]; then
        echo -e "${CYAN}方式3: 使用 Launch 文件启动${NC}"
        ros2 launch fishbot_microros_voice llm_enhanced_voice_system.launch.py \
            server_url:=${ECHOKIT_SERVER_URL} \
            log_level:=info
    else
        # 方式4: 回退到改进版 Python 脚本
        if [[ -f "improved_voice_control.py" ]]; then
            echo -e "${CYAN}方式4: 使用改进版 Python 脚本${NC}"
            python3 improved_voice_control.py
        else
            echo -e "${RED}❌ 未找到可用的启动方式${NC}"
            echo -e "${CYAN}💡 请检查以下文件是否存在:${NC}"
            echo -e "${CYAN}   - $EXECUTABLE_PATH${NC}"
            echo -e "${CYAN}   - $LAUNCH_FILE${NC}"
            echo -e "${CYAN}   - improved_voice_control.py${NC}"
            exit 1
        fi
    fi
fi

echo ""
echo -e "${GREEN}👋 Enhanced LLM Voice Control System 已停止${NC}"